"""
舵机控制服务器
提供机器人舵机控制相关的MCP工具
"""

from typing import Dict, Any, List, Optional
from mcp.server.fastmcp import FastMCP
from robot_core.servo_controller import servo_controller
from robot_core.servos.pwm_servo import PWMServo


class ServoServer(FastMCP):
    """舵机控制服务器类"""

    def __init__(self):
        """初始化舵机控制服务器"""
        super().__init__("servo_server")

        # 注册工具
        self._register_tools()

    def _register_tools(self):
        """注册所有舵机控制工具"""

        @self.tool()
        async def set_servo_angle(
            servo_id: int, angle: float, speed: Optional[float] = None
        ) -> Dict[str, Any]:
            """设置单个舵机的角度

            Args:
                servo_id: 舵机ID
                angle: 目标角度(0-180度)
                speed: 运动速度(0-1,可选)

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                # 将角度转换为舵机位置(0-1000)
                position = int(angle * 1000 / 180)
                # 将速度转换为舵机速度(0-1000)
                servo_speed = int(speed * 1000) if speed else 1000

                success = servo_controller.set_position(servo_id, position, servo_speed)
                if success:
                    return {
                        "status": "success",
                        "message": f"Set servo {servo_id} to {angle} degrees"
                        + (f" at speed {speed}" if speed else ""),
                    }
                else:
                    return {
                        "status": "error",
                        "message": f"Failed to set servo {servo_id} position",
                    }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def set_servos_angles(
            servo_angles: Dict[int, float], speed: Optional[float] = None
        ) -> Dict[str, Any]:
            """设置多个舵机的角度

            Args:
                servo_angles: 舵机ID到角度的映射
                speed: 运动速度(0-1,可选)

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                results = {}
                servo_speed = int(speed * 1000) if speed else 1000

                for servo_id, angle in servo_angles.items():
                    position = int(angle * 1000 / 180)
                    success = servo_controller.set_position(
                        servo_id, position, servo_speed
                    )
                    results[servo_id] = "success" if success else "failed"

                return {
                    "status": "success",
                    "message": f"Set servos angles with results: {results}",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def get_servo_angle(servo_id: int) -> Dict[str, Any]:
            """获取单个舵机的当前角度

            Args:
                servo_id: 舵机ID

            Returns:
                Dict[str, Any]: 舵机角度信息
            """
            try:
                position = servo_controller.get_position(servo_id)
                if position is not None:
                    angle = position * 180 / 1000
                    return {
                        "status": "success",
                        "servo_id": servo_id,
                        "angle": angle,
                        "position": position,
                    }
                else:
                    return {
                        "status": "error",
                        "message": f"Failed to get servo {servo_id} position",
                    }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def get_servos_angles(servo_ids: List[int]) -> Dict[str, Any]:
            """获取多个舵机的当前角度

            Args:
                servo_ids: 舵机ID列表

            Returns:
                Dict[str, Any]: 舵机角度信息
            """
            try:
                angles = {}
                positions = {}

                for servo_id in servo_ids:
                    position = servo_controller.get_position(servo_id)
                    if position is not None:
                        angles[servo_id] = position * 180 / 1000
                        positions[servo_id] = position

                return {"status": "success", "angles": angles, "positions": positions}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def set_servo_parameters(
            servo_id: int, parameters: Dict[str, Any]
        ) -> Dict[str, Any]:
            """设置舵机参数

            Args:
                servo_id: 舵机ID
                parameters: 参数配置
                    - min_angle: 最小角度
                    - max_angle: 最大角度
                    - offset: 角度偏移
                    - speed_limit: 速度限制

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                # TODO: 实现舵机参数设置
                # 需要扩展ServoControllerInterface以支持参数设置
                return {
                    "status": "success",
                    "message": f"Set servo {servo_id} parameters: {parameters}",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def get_servo_parameters(servo_id: int) -> Dict[str, Any]:
            """获取舵机参数

            Args:
                servo_id: 舵机ID

            Returns:
                Dict[str, Any]: 舵机参数信息
            """
            try:
                # TODO: 实现舵机参数获取
                # 需要扩展ServoControllerInterface以支持参数获取
                return {
                    "status": "success",
                    "servo_id": servo_id,
                    "parameters": {
                        "min_angle": 0,
                        "max_angle": 180,
                        "offset": 0,
                        "speed_limit": 1.0,
                    },
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def emergency_stop() -> Dict[str, Any]:
            """紧急停止所有舵机

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                servo_controller.emergency_stop()
                return {"status": "success", "message": "All servos emergency stopped"}
            except Exception as e:
                return {"status": "error", "message": str(e)}
